/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_

#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/point_grid.h>
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/voxel_grid_model.h>
#include <base_local_planner/trajectory_planner.h>
#include <base_local_planner/map_grid_visualizer.h>

#include <base_local_planner/planar_laser_scan.h>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>

#include <tf2_ros/buffer.h>

#include <boost/thread.hpp>

#include <string>

#include <angles/angles.h>

#include <nav_core/base_local_planner.h>

#include <dynamic_reconfigure/server.h>
#include <base_local_planner/BaseLocalPlannerConfig.h>

#include <base_local_planner/odometry_helper_ros.h>

namespace base_local_planner
{
/** 循环检查是否到达目标点位置（给定位置误差范围内），若未到达，则调用TrajectoryPlanner类函数来进行局部路径规划，得到下一步速度，反馈给Movebase；
 *  若已到达，则检查是否到达目标姿态，若未到达，先给机器人降速至阈值内，再使它原地旋转，直至达到目标姿态（给定姿态误差范围内），至此局部规划器完成任务。
 * @class TrajectoryPlannerROS
 * @brief A ROS wrapper for the trajectory controller that queries the param server to construct a controller
 */
class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner
{
public:
    /**
     * @brief  Default constructor for the ros wrapper 默认构造函数
     */
    TrajectoryPlannerROS() :
            world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL),
            setup_(false), initialized_(false), odom_helper_("odom") {}

    /**
     * @brief  Constructs the ros wrapper 构造ros封装类
     * @param name The name to give this instance of the trajectory planner 局部轨迹规划器的名字
     * @param tf A pointer to a transform listener
     * @param costmap The cost map to use for assigning costs to trajectories 代价地图
     */
    TrajectoryPlannerROS(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros) :
            world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), setup_(false), initialized_(false),
            odom_helper_("odom") { initialize(name, tf, costmap_ros); }

    /** 这个函数的主要工作是从参数服务器下载参数值给局部规划器赋参，首先设置全局和局部规划结果的发布，并用传入的参数costmap_ros来初始化局部规划器用到的代价地图。
     * @brief  Constructs the ros wrapper 构造ros封装类
     * @param name The name to give this instance of the trajectory planner 局部轨迹规划器的名字
     * @param tf A pointer to a transform listener
     * @param costmap The cost map to use for assigning costs to trajectories ROS的地图封装类，它整合了静态层、障碍层、膨胀层地图
     */
    void initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros);

    /**
     * @brief  Destructor for the wrapper 封装类的析构函数
     */
    ~TrajectoryPlannerROS();

    /** 该函数在Movebase的executeCycle函数中被调用，executeCycle函数本身是被循环执行的，
     * 所以能够不断进行局部速度规划，从而获得连续的速度指令，控制机器人行动。
     *
     * @brief  Given the current position, orientation, and velocity of the robot, 根据当前机器人坐标，朝向和速度，计算下个下发速度
     * compute velocity commands to send to the base
     * @param cmd_vel Will be filled with the velocity command to be passed to the robot base 存储者下发速度
     * @return True if a valid trajectory was found, false otherwise  True意味着找到有效轨迹
     */
    bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel);

    /** Movebase通过调用这个函数传入先前针对当前位置和目标点间规划好的全局路径，与全局路径的贴合程度将作为局部规划路线的一个打分项。
     * @brief  Set the plan that the controller is following  更新局部规划器要跟随的路径
     * @param orig_global_plan The plan to pass to the controller  更新的路径
     * @return True if the plan was updated successfully, false otherwise  表示更新成功
     */
    bool setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan);

    /** 判断是否到达目标点
     * @brief  Check if the goal pose has been achieved
     * @return True if achieved, false otherwise
     */
    bool isGoalReached();

    /** 计算轨迹并且为其打分
     * @brief  Generate and score a single trajectory
     * @param vx_samp The x velocity used to seed the trajectory 用于推算出路径的x方向速度
     * @param vy_samp The y velocity used to seed the trajectory 用于推算出路径的y方向速度
     * @param vtheta_samp The theta velocity used to seed the trajectory 用于推算出路径的角速度
     * @param update_map Whether or not to update the map for the planner 是否更新地图
     * when computing the legality of the trajectory, this is useful to set
     * to false if you're going to be doing a lot of trajectory checking over
     * a short period of time
     * @return True if the trajectory is legal, false otherwise
     */
    bool checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);

    /** 产生和打分单个局部轨迹
     * @brief  Generate and score a single trajectory
     * @param vx_samp The x velocity used to seed the trajectory 用于推算出路径的x方向速度
     * @param vy_samp The y velocity used to seed the trajectory 用于推算出路径的y方向速度
     * @param vtheta_samp The theta velocity used to seed the trajectory 用于推算出路径的角速度
     * @param update_map Whether or not to update the map for the planner 是否更新地图
     * when computing the legality of the trajectory, this is useful to set
     * to false if you're going to be doing a lot of trajectory checking over
     * a short period of time
     * @return score of trajectory (double)
     */
    double scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);

    bool isInitialized() { return initialized_; }

    /** @brief Return the inner TrajectoryPlanner object.  Only valid after initialize(). */
    // 返回内部的TrajectoryPlanner对象，只有在initialize()后才有效
    TrajectoryPlanner *getPlanner() const { return tc_; }

private:
    /** 用于更新局部规划器参数的回调函数
     * @brief Callback to update the local planner's parameters based on dynamic reconfigure
     */
    void reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level);

    /** 在达到目标点误差范围内，且速度降至极小后，最后一步的工作是原地旋转至目标姿态。
     *  它采用一种类似“反馈控制”的思想，通过计算当前姿态与目标姿态的差值，通过这个差值来控制下一步的角速度。
     * @brief Once a goal position is reached... rotate to the goal orientation
     * @param  global_pose The pose of the robot in the global frame 机器人在全局坐标系下的位姿
     * @param  robot_vel The velocity of the robot  机器人速度
     * @param  goal_th The desired th value for the goal 期望的th 速度
     * @param  cmd_vel The velocity commands to be filled 速度命令的载体
     * @return  True if a valid trajectory was found, false otherwise 代表找到有效的局部路径规划
     */
    bool rotateToGoal(const geometry_msgs::PoseStamped &global_pose, const geometry_msgs::PoseStamped &robot_vel,
                      double goal_th, geometry_msgs::Twist &cmd_vel);

    /** 机器人已达目标附近范围而姿态未达姿态要求时，在调整姿态前，将机器人速度降至阈值以下。
     *  将下一步速度设置为当前速度以最大反向加速度在一个仿真周期sim_period_内降至的速度，角速度同理。注意在计算中防止越过0界。
     * @brief Stop the robot taking into account acceleration limits
     * @param  global_pose The pose of the robot in the global frame 机器人在全局坐标系下的位姿
     * @param  robot_vel The velocity of the robot 机器人速度
     * @param  cmd_vel The velocity commands to be filled 速度命令的载体
     * @return  True if a valid trajectory was found, false otherwise True 代表找到有效的局部路径规划
     */
    bool stopWithAccLimits(const geometry_msgs::PoseStamped &global_pose,
            const geometry_msgs::PoseStamped &robot_vel, geometry_msgs::Twist &cmd_vel);

    std::vector<double> loadYVels(ros::NodeHandle node);

    double sign(double x) { return x < 0.0 ? -1.0 : 1.0; }

    WorldModel *world_model_; ///< @brief The world model that the controller will use 局部规划器用到的世界模型
    TrajectoryPlanner *tc_; ///< @brief The trajectory controller 轨迹局部规划器，tc是 trajectory controller的缩写
    // 局部规划器使用的关于代价地图的ROS封装类
    costmap_2d::Costmap2DROS *costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
    // 局部规划器用到的代价地图
    costmap_2d::Costmap2D *costmap_; ///< @brief The costmap the controller will use
    // 地图网格可视化代价地图产生的势场
    MapGridVisualizer map_viz_; ///< @brief The map grid visualizer for outputting the potential field generated by the cost function
    // 用于点云转换
    tf2_ros::Buffer *tf_; ///< @brief Used for transforming point clouds
    // 全局坐标系
    std::string global_frame_; ///< @brief The frame in which the controller will run
    // 传感器的有效感知距离
    double max_sensor_range_; ///< @brief Keep track of the effective maximum range of our sensors
    // 用于得到机器人的速度
    nav_msgs::Odometry base_odom_; ///< @brief Used to get the velocity of the robot
    // 机器人的基坐标系
    std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
    double rot_stopped_velocity_, trans_stopped_velocity_;
    double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_;
    std::vector<geometry_msgs::PoseStamped> global_plan_;
    bool prune_plan_;
    boost::recursive_mutex odom_lock_;
    double max_vel_th_, min_vel_th_;
    double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
    double sim_period_;
    bool rotating_to_goal_;
    bool reached_goal_;
    bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
    ros::Publisher g_plan_pub_, l_plan_pub_;
    dynamic_reconfigure::Server<BaseLocalPlannerConfig> *dsrv_;
    base_local_planner::BaseLocalPlannerConfig default_config_;
    bool setup_;
    bool initialized_;
    base_local_planner::OdometryHelperRos odom_helper_;
    std::vector<geometry_msgs::Point> footprint_spec_;
};
};
#endif
